import os
import sys

sys.path.append(".")

import math
import numpy as np
# import converter
import matplotlib
matplotlib.use('TkAgg')
from matplotlib import pyplot as plt
from matplotlib.lines import Line2D
from mpl_toolkits.mplot3d import Axes3D
from mpl_toolkits.mplot3d.art3d import Line3D
import time_processor as tp
import geo_processor as gp
import math_processor as mp
from scipy import interpolate

# evo_ape tum /shared/VIO/openvslam-main/build/trajectory_gt.txt /shared/VIO/openvslam-main/build/trajectory_50000_65000_new.txt --t_max_diff  -va --plot --plot_mode xz --save_results results/SPTAM.zip --align --correct_scale

def read_ins_data(ins_path):

    result = []

    num = 0
    with open(ins_path) as f:
        for line in f:
            if line[0] != '#':
                pass
            num = num + 1 
            items = line.split(' ')
    
            result1 = []
            result1.append(int(items[0])) # timestamp
            result1.append(float(items[1])) # lat
            result1.append(float(items[2])) # lon
            result1.append(float(items[3])) # hei
            result1.append(float(items[4])) # Roll
            result1.append(float(items[5])) # Pitch
            result1.append(float(items[6])) # Yaw

            result.append(result1)

    return np.array(result)


def read_gps_data(gps_path):

    result = []

    num = 0
    with open(gps_path) as f:
        for line in f:
            if line[0] != '#':
                pass
            num = num + 1 
            items = line.split(' ')
    
            result1 = []
            result1.append(int(items[0])) # timestamp
            result1.append(float(items[1])) # lat
            result1.append(float(items[2])) # lon
            result1.append(float(items[3])) # hei

            result.append(result1)

    return np.array(result)


def change_ins_to_gt_and_save(ins_result, gps_org, save_path):
    output_gt = open(save_path, "w+")
    
    for i in range(ins_result.shape[0]):

        e, n, u = gp.geodetic_to_enu(ins_result[i, 1], ins_result[i, 2], ins_result[i, 3], gps_org[0], gps_org[1], gps_org[2])
        cnb = mp.rpy2RotCnb(ins_result[i, 4], ins_result[i, 5], ins_result[i, 6])
        q_cnb = mp.rot2quat(cnb)

        output_gt.write("{:.0f}".format(ins_result[i,0]))
        output_gt.write(" ")
        output_gt.write("{:.12f}".format(e))
        output_gt.write(" ")
        output_gt.write("{:.12f}".format(n))
        output_gt.write(" ")
        output_gt.write("{:.12f}".format(u))
        output_gt.write(" ")
        output_gt.write("{:.12f}".format(q_cnb[0]))
        output_gt.write(" ")
        output_gt.write("{:.12f}".format(q_cnb[1]))
        output_gt.write(" ")
        output_gt.write("{:.12f}".format(q_cnb[2]))
        output_gt.write(" ")
        output_gt.write("{:.12f}".format(q_cnb[3]))
        output_gt.write("\n")

    output_gt.close()

def change_gps_to_gt_and_save(gps_result, gps_org, save_path):
    output_gt = open(save_path, "w+")
    
    for i in range(gps_result.shape[0]):

        e, n, u = gp.geodetic_to_enu(gps_result[i, 1], gps_result[i, 2], gps_result[i, 3], gps_org[0], gps_org[1], gps_org[2])

        output_gt.write("{:.0f}".format(gps_result[i,0]))
        output_gt.write(" ")
        output_gt.write("{:.12f}".format(e))
        output_gt.write(" ")
        output_gt.write("{:.12f}".format(n))
        output_gt.write(" ")
        output_gt.write("{:.12f}".format(u))
        output_gt.write(" ")
        output_gt.write("{:.12f}".format(0))
        output_gt.write(" ")
        output_gt.write("{:.12f}".format(0))
        output_gt.write(" ")
        output_gt.write("{:.12f}".format(0))
        output_gt.write(" ")
        output_gt.write("{:.12f}".format(1))
        output_gt.write("\n")

    output_gt.close()

if __name__ == "__main__":
    
    gps_org = [30.61, 114.25, 23.7]

    path1 = r"/shared/20210902/data/data/ins.txt"
    save_path1 = r"/shared/VIO/openvslam-main/build/trajectory_gt.txt"
    ins_result = read_ins_data(path1)
    change_ins_to_gt_and_save(ins_result, gps_org, save_path1)

    # path2 = r"/media/cjg/Elements/pano20210902/pro2/data/gps_new.txt"
    # save_path2 = r"/shared/VIO/openvslam-main/build/trajectory_gt2.txt"
    # gps_result = read_gps_data(path2)
    # change_gps_to_gt_and_save(gps_result, gps_org, save_path2)
    
